package pheromoneMap;

import java.io.PrintStream;
import java.util.Map;
import java.util.EnumMap;
import java.util.Collections;

public class PheromoneMap {
	static int n_pheromoneTypes = 0;
	
	/**
	 * m_pheromoneGrid: 3D array of Vector2D objects, where the first and
	 * second indexes are the X and Y coordinate of the box and the third
	 * index is the type of pheromone.  Int m_gridSize represents the size
	 * of each box on a side, and is used to translate between game
	 * coordinates and the proper box.
	 **/
	//private Vector2D m_pheromoneGrid[][][];
	public Map<PheromoneType, Vector2D[][]> m_pheromoneMap;
	public int m_mapWidth, m_mapHeight;

	/**
	 * decayRate and diffuseRate: Array of doubles where each entry
	 * represents the decay or diffusion rate of the pheromone type
	 * corresponding to the array index.
	 **/
/*	static Map<PheromoneType, double> decayRate;
	static Map<PheromoneType, double> diffuseRate;
	static Map<PheromoneType, double> coalesceRate;
*/
	/**
	 * Constructor: Takes in a grid size and a map width and height.
	 * Assigns memory to the vectors and such.
	 **/
	public PheromoneMap( int mapWidth, int mapHeight ) {
		m_pheromoneMap = Collections.synchronizedMap(new EnumMap <PheromoneType, Vector2D[][]>(PheromoneType.class));
		n_pheromoneTypes = PheromoneType.values().length;
		for (PheromoneType type : PheromoneType.values()) {
			Vector2D[][] vec2D = new Vector2D[mapWidth][mapHeight];
			//System.out.println("type: " + type + " - vale: " + m_pheromoneMap.get(type));
			for( int j = 0; j < mapWidth; j++ )
				for( int k = 0; k < mapHeight; k++ )
					vec2D[j][k] = new Vector2D();
			m_pheromoneMap.put(type, vec2D);
		
		}
		
		//m_pheromoneGrid = new Vector2D[mapWidth][mapHeight][n_pheromoneTypes];
		m_mapWidth = mapWidth;
		m_mapHeight = mapHeight;
		
		/*for( int i = 0; i < mapWidth; i++ )
			for( int j = 0; j < mapHeight; j++ )
				for( int k = 0; k < n_pheromoneTypes; k++ )
					m_pheromoneGrid[i][j][k] = new Vector2D();
		
		decayRate = new double[n_pheromoneTypes];
		diffuseRate = new double[n_pheromoneTypes];
		coalesceRate = new double[n_pheromoneTypes];
		
		for( int i = 0; i < n_pheromoneTypes; i++ ) {
			decayRate[i] = 0.999;
			diffuseRate[i] = 0.9985;
			coalesceRate[i] = 0.99965;
		}
		*/
	}

	public void setDiffuse(PheromoneType type, double rate)
	{
		type.setDiffuseRate(rate);
	}
	
	public void setDecay(PheromoneType type, double rate)
	{
		type.setDecayRate(rate);
	}
	
	public void setCoalesce(PheromoneType type, double rate)
	{
		type.setCoalesceRate(rate);
	}
	
	/**
	 * When an ant calls placePheromone, find the vector it should be
	 * interacting with based on its xPos and yPos and the type, then
	 * add a new vector represented by strength and direction.
	 **/
	public void placePheromone( int xPos, int yPos, PheromoneType type, double strength,
			double direction ) {
		if( xPos >= 0 && xPos < m_mapWidth && yPos >= 0 && yPos < m_mapHeight ) {
			double xComponent = Math.cos( direction ) * strength;
			double yComponent = Math.sin( direction ) * strength;
			Vector2D pheromoneVector = new Vector2D( xComponent, yComponent );
			m_pheromoneMap.get(type)[xPos][yPos].update(
					m_pheromoneMap.get(type)[xPos][yPos].add( pheromoneVector ) );
		}
	}
	public void placeRepulsionPheromone( int xPos, int yPos, PheromoneType type, double strength ) {
		if( xPos >= 0 && xPos < m_mapWidth && yPos >= 0 && yPos < m_mapHeight ) {
			if( xPos > 0 ) {
				Vector2D leftVector = new Vector2D( -strength, 0 );
				m_pheromoneMap.get(type)[xPos-1][yPos].update(
						m_pheromoneMap.get(type)[xPos-1][yPos].add( leftVector ) );
			}
			if( yPos > 0 ) {
				Vector2D upVector = new Vector2D( 0, -strength );
				m_pheromoneMap.get(type)[xPos][yPos-1].update(
						m_pheromoneMap.get(type)[xPos][yPos-1].add( upVector ) );
			}
			if( xPos < m_mapWidth - 1 ) {
				Vector2D rightVector = new Vector2D( strength, 0 );
				m_pheromoneMap.get(type)[xPos+1][yPos].update(
						m_pheromoneMap.get(type)[xPos+1][yPos].add( rightVector ) );
			}
			if( yPos < m_mapHeight - 1 ) {
				Vector2D downVector = new Vector2D( 0, strength );
				m_pheromoneMap.get(type)[xPos][yPos+1].update(
						m_pheromoneMap.get(type)[xPos][yPos+1].add( downVector ) );
			}
			m_pheromoneMap.get(type)[xPos][yPos].update( new Vector2D( 0.0, 0.0 ) );
		}
	}
	
	public void placeRepulsionSquare(int xPos, int yPos, PheromoneType type, double strength, int width, int height) {
		if( xPos >= 0 && xPos < m_mapWidth && yPos >= 0 && yPos < m_mapHeight ) {
			// along the top and bottom
			Vector2D upVector = new Vector2D( 0, -strength );
			Vector2D downVector = new Vector2D( 0, strength );
			boolean heightHitsInMap = true;
			if (yPos + height >= m_mapHeight) {
				heightHitsInMap = false;
			}
			
			//top left is pointing to the diagonal top left
			m_pheromoneMap.get(type)[xPos][yPos].update(
					m_pheromoneMap.get(type)[xPos][yPos].add( new Vector2D( -strength, -strength ) ) );
			//bottom left is pointing to the diagonal bottom left
			if (heightHitsInMap) {
				m_pheromoneMap.get(type)[xPos][yPos + height].update(
					m_pheromoneMap.get(type)[xPos][yPos + height].add( new Vector2D( -strength, strength ) ) );
			}
			for (int countX = xPos + 1; countX < (xPos + width); countX++) {		
				if (xPos + countX > m_mapWidth) { 
					//we have hit the edge of the map
					break;
				} else {
					m_pheromoneMap.get(type)[countX][yPos].update(
							m_pheromoneMap.get(type)[countX][yPos].add( upVector ) );
					if (heightHitsInMap) {
					m_pheromoneMap.get(type)[countX][yPos + height].update(
							m_pheromoneMap.get(type)[countX][yPos + height].add( downVector ) );
					
					}
				}
				
			}
			
			//along the right and left side
			Vector2D rightVector = new Vector2D( strength, 0 );
			Vector2D leftVector = new Vector2D( -strength, 0 );
			boolean widthFitsInMap = false;
			if (xPos + width < m_mapWidth) {
				widthFitsInMap = true;
			} 
			//top right is pointing to the diagonal top right
			if (widthFitsInMap) {
				m_pheromoneMap.get(type)[xPos + width][yPos].update(
					m_pheromoneMap.get(type)[xPos + width][yPos].add( new Vector2D( strength, -strength ) ) );
			//bottom right is pointing to the diagonal bottom right
			}
			if (heightHitsInMap && widthFitsInMap) {
				m_pheromoneMap.get(type)[xPos + width][yPos + height].update(
					m_pheromoneMap.get(type)[xPos + width][yPos + height].add( new Vector2D( strength, strength ) ) );
			}
			for (int countY = yPos + 1; countY < (yPos + height); countY++) {
				
				if (yPos + countY > m_mapHeight) {
					//we have hit the bottom of the map
					break;
				} else {
					
					m_pheromoneMap.get(type)[xPos][countY].update(
							m_pheromoneMap.get(type)[xPos][countY].add( leftVector ) );

					if (widthFitsInMap) {
						m_pheromoneMap.get(type)[xPos + width][countY].update(
							m_pheromoneMap.get(type)[xPos + width][countY].add( rightVector ) );
					}
				}
			}
			
		}
	}
	
	public void placeAttractionPheromone( int xPos, int yPos, PheromoneType type, double strength ) {
		if( xPos >= 0 && xPos < m_mapWidth && yPos >= 0 && yPos < m_mapHeight ) {
			if( xPos > 0 ) {
				Vector2D leftVector = new Vector2D( strength, 0 );
				m_pheromoneMap.get(type)[xPos-1][yPos].update(
						m_pheromoneMap.get(type)[xPos-1][yPos].add( leftVector ) );
			}
			if( yPos > 0 ) {
				Vector2D upVector = new Vector2D( 0, strength );
				m_pheromoneMap.get(type)[xPos][yPos-1].update(
						m_pheromoneMap.get(type)[xPos][yPos-1].add( upVector ) );
			}
			if( xPos < m_mapWidth - 1 ) {
				Vector2D rightVector = new Vector2D( -strength, 0 );
				m_pheromoneMap.get(type)[xPos+1][yPos].update(
						m_pheromoneMap.get(type)[xPos+1][yPos].add( rightVector ) );
			}
			if( yPos < m_mapHeight - 1 ) {
				Vector2D downVector = new Vector2D( 0, -strength );
				m_pheromoneMap.get(type)[xPos][yPos+1].update(
						m_pheromoneMap.get(type)[xPos][yPos+1].add( downVector ) );
			}
			m_pheromoneMap.get(type)[xPos][yPos].update( new Vector2D( 0.0, 0.0 ) );
		}
	}
	public void placeAttractionSquare(int xPos, int yPos, PheromoneType type, double strength, int width, int height) {
		if( xPos >= 0 && xPos < m_mapWidth && yPos >= 0 && yPos < m_mapHeight ) {
			// along the top and bottom
			Vector2D upVector = new Vector2D( 0, strength );
			Vector2D downVector = new Vector2D( 0, -strength );
			boolean heightHitsInMap = true;
			if (yPos + height >= m_mapHeight) {
				heightHitsInMap = false;
			}
			
			//top left is pointing to the diagonal top left
			m_pheromoneMap.get(type)[xPos][yPos].update(
					m_pheromoneMap.get(type)[xPos][yPos].add( new Vector2D( strength, strength ) ) );
			//bottom left is pointing to the diagonal bottom left
			if (heightHitsInMap) {
				m_pheromoneMap.get(type)[xPos][yPos + height].update(
					m_pheromoneMap.get(type)[xPos][yPos + height].add( new Vector2D( strength, -strength ) ) );
			}
			for (int countX = xPos + 1; countX < (xPos + width); countX++) {		
				if (xPos + countX > m_mapWidth) { 
					//we have hit the edge of the map
					break;
				} else {
					m_pheromoneMap.get(type)[countX][yPos].update(
							m_pheromoneMap.get(type)[countX][yPos].add( upVector ) );
					if (heightHitsInMap) {
					m_pheromoneMap.get(type)[countX][yPos + height].update(
							m_pheromoneMap.get(type)[countX][yPos + height].add( downVector ) );
					
					}
				}
				
			}
			
			//along the right and left side
			Vector2D rightVector = new Vector2D( -strength, 0 );
			Vector2D leftVector = new Vector2D( strength, 0 );
			boolean widthFitsInMap = false;
			if (xPos + width < m_mapWidth) {
				widthFitsInMap = true;
			} 
			//top right is pointing to the diagonal top right
			if (widthFitsInMap) {
				m_pheromoneMap.get(type)[xPos + width][yPos].update(
					m_pheromoneMap.get(type)[xPos + width][yPos].add( new Vector2D( -strength, strength ) ) );
			//bottom right is pointing to the diagonal bottom right
			}
			if (heightHitsInMap && widthFitsInMap) {
				m_pheromoneMap.get(type)[xPos + width][yPos + height].update(
					m_pheromoneMap.get(type)[xPos + width][yPos + height].add( new Vector2D( -strength, -strength ) ) );
			}
			for (int countY = yPos + 1; countY < (yPos + height); countY++) {
				
				if (yPos + countY > m_mapHeight) {
					//we have hit the bottom of the map
					break;
				} else {
					
					m_pheromoneMap.get(type)[xPos][countY].update(
							m_pheromoneMap.get(type)[xPos][countY].add( leftVector ) );

					if (widthFitsInMap) {
						m_pheromoneMap.get(type)[xPos + width][countY].update(
							m_pheromoneMap.get(type)[xPos + width][countY].add( rightVector ) );
					}
				}
			}
			
		}
	}
	

	/**
	 * getMagnitude and getDirection should first translate from the ant's
	 * xPos and yPos to X and Y indexes then return the magnitude or
	 * direction of the vector at that point.
	 **/
	public double getMagnitude( int xPos, int yPos, PheromoneType type ) {
		return m_pheromoneMap.get(type)[xPos][yPos].logMagnitude();
	}
	public double getDirection( int xPos, int yPos, PheromoneType type ) {
		return m_pheromoneMap.get(type)[xPos][yPos].direction();
	}
	public Vector2D getVector( int xPos, int yPos, PheromoneType type ) {
		return m_pheromoneMap.get(type)[xPos][yPos];
	}

	/**
	 * decay and diffuse: Decay causes the pheromones to decay at a
	 * particular rate based on the type.  Diffuse causes the pheromones
	 * to Gaussian blur with their neighbors.  In both cases, pass the
	 * number of time-ticks that have passed, and do that number of
	 * iterations.
	 **/
	public void decay( PheromoneType type, int deltaT ) {
		double decay = Math.pow( type.getDecayRate(), deltaT );
		
		for( int xPos = 0; xPos < m_mapWidth; xPos++ ) {
			for( int yPos = 0; yPos < m_mapHeight; yPos++ )		{		
				m_pheromoneMap.get(type)[xPos][yPos].update(
					m_pheromoneMap.get(type)[xPos][yPos].multiply( decay ) );
			}
		}
	}
	

	public void diffuse( PheromoneType type, int deltaT ) {
		double diffuse = Math.pow( type.getDiffuseRate(), deltaT );
		Vector2D weightedGaussianSum, weightedSelf;
		double numNeighbors;
		
		Vector2D[][] cloneMap = new Vector2D[m_mapWidth][m_mapHeight];
		
		for( int i = 0; i < m_mapWidth; i++ )
			for( int j = 0; j < m_mapHeight; j++ )
				cloneMap[i][j] = new Vector2D( m_pheromoneMap.get(type)[i][j] );
		
		for( int i = 0; i < m_mapWidth; i++ )
			for( int j = 0; j < m_mapHeight; j++ ) {
//				System.out.println( "Diffusing on cell " + i + "," + j );
//				System.out.println( "Cloned value is " + cloneMap[i][j].magnitude() );
					weightedSelf = cloneMap[i][j].multiply( diffuse );
					weightedGaussianSum = new Vector2D();
					numNeighbors = 0;
					
					if( i > 0 ) {
//						System.out.println( "Adding neighbor " + (i-1) + "," + j + ": " + cloneMap[i-1][j].magnitude() );
						weightedGaussianSum.update( weightedGaussianSum.add( cloneMap[i-1][j] ) );
						numNeighbors += 1;
					}
					if( j > 0 ) {
//						System.out.println( "Adding neighbor " + i + "," + (j-1) + ": " + cloneMap[i][j-1].magnitude() );
						weightedGaussianSum.update( weightedGaussianSum.add( cloneMap[i][j-1] ) );
						numNeighbors += 1;
					}
					if( j < m_mapHeight - 1 ) {
//						System.out.println( "Adding neighbor " + i + "," + (j+1) + ": " + cloneMap[i][j+1].magnitude() );
						weightedGaussianSum.update( weightedGaussianSum.add( cloneMap[i][j+1] ) );
						numNeighbors += 1;
					}
					if( i < m_mapWidth - 1 ) {
//						System.out.println( "Adding neighbor " + (i+1) + "," + j + ": " + cloneMap[i+1][j].magnitude() );
						weightedGaussianSum.update( weightedGaussianSum.add( cloneMap[i+1][j] ) );
						numNeighbors += 1;
					}
					weightedGaussianSum.update( weightedGaussianSum.add(
							cloneMap[i][j].multiply( numNeighbors ) ) );
					
//					System.out.println( "Neighbor sum is " + weightedGaussianSum.magnitude() );

					weightedGaussianSum.update(
							weightedGaussianSum.multiply( 1 / ( numNeighbors * 2 ) ) );
					weightedGaussianSum.update(
							weightedGaussianSum.multiply( 1 - diffuse ) );
					
//					System.out.println( "Weighted self component is " + weightedSelf.magnitude() );
//					System.out.println( "Weighted neighbor component is " + weightedGaussianSum.magnitude() );

//					System.out.println( "Before update, it is " + m_pheromoneMap.get(type)[i][j].magnitude());
					m_pheromoneMap.get(type)[i][j].update( weightedSelf.add( weightedGaussianSum ) );
//					System.out.println( "After update, it is " + m_pheromoneMap.get(type)[i][j].magnitude());
			}
	}
	
	public void coalesce( PheromoneType type, int deltaT ) {
		double coalesce = Math.pow( type.getCoalesceRate(), deltaT );
		Vector2D weightedCoalesceSum, weightedSelf;
		double numNeighbors;
		
		Vector2D[][] cloneMap = new Vector2D[m_mapWidth][m_mapHeight];
		
		for( int i = 0; i < m_mapWidth; i++ )
			for( int j = 0; j < m_mapHeight; j++ )
				cloneMap[i][j] = new Vector2D( m_pheromoneMap.get(type)[i][j] );
		
		for( int i = 0; i < m_mapWidth; i++ )
			for( int j = 0; j < m_mapHeight; j++ ) {
					weightedSelf = cloneMap[i][j].multiply( coalesce );
					weightedCoalesceSum = new Vector2D();
					numNeighbors = 0;
	
					if( i > 0 && j > 0) {
						Vector2D unitVector = new Vector2D( -1, -1 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i-1][j-1].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( i > 0 ) {
						Vector2D unitVector = new Vector2D( -1, 0 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i-1][j].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( i > 0 && j < m_mapHeight - 1 ) {
						Vector2D unitVector = new Vector2D( -1, 1 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i-1][j+1].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( j > 0 ) {
						Vector2D unitVector = new Vector2D( 0, -1 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i][j-1].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( j < m_mapHeight - 1 ) {
						Vector2D unitVector = new Vector2D( 0, 1 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i][j+1].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( i < m_mapWidth - 1 && j > 0) {
						Vector2D unitVector = new Vector2D( 1, -1 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i+1][j-1].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( i < m_mapWidth - 1 ) {
						Vector2D unitVector = new Vector2D( 1, 0 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i+1][j].magnitude() ) ) );
						numNeighbors += 1;
					}
					if( i < m_mapWidth - 1 && j < m_mapHeight - 1 ) {
						Vector2D unitVector = new Vector2D( 1, 1 );
						unitVector.update( unitVector.multiply( 1 / unitVector.magnitude() ) );
						
						weightedCoalesceSum.update( weightedCoalesceSum.add(
								unitVector.multiply( cloneMap[i+1][j+1].magnitude() ) ) );
						numNeighbors += 1;
					}
					
					weightedCoalesceSum.update(
							weightedCoalesceSum.multiply( 1 / ( numNeighbors ) ) );
					weightedCoalesceSum = weightedCoalesceSum.multiply( 1 - coalesce );
					
					m_pheromoneMap.get(type)[i][j].update( weightedSelf.add( weightedCoalesceSum ) );
			
		}
	}
	
	
	public void printMap( PheromoneType type ) {
		printMap(type, this.m_mapWidth, this.m_mapHeight, 0, 0);
	}
	
	public void printMap( PheromoneType type, int width, int height, int xCenter, int yCenter ) {
		PrintStream ps = new PrintStream( System.out );
		
		int lowWidth, highWidth;
		int lowHeight, highHeight;
		
		lowWidth = xCenter - width;
		lowWidth = (lowWidth < 0)? 0 : lowWidth;
		highWidth = xCenter + width;
		highWidth = (highWidth > m_mapWidth)? m_mapWidth : highWidth;
		lowHeight = yCenter - height;
		lowHeight = (lowHeight < 0)? 0 : lowHeight;
		highHeight = yCenter + height;
		highHeight = (highHeight > m_mapHeight)? m_mapHeight : highHeight;
		
		
		for( int i = lowWidth; i < highWidth; i++ ) {
			for( int j = lowHeight; j < highHeight; j++ )
				ps.printf( "%1.4f@%1.4f  ", getMagnitude( i, j, type ),
						getDirection( i, j, type ) );
			ps.println();
		}
	}

}
